#include "PolynomialPath.h"
using namespace Math;

void PolynomialFunction::PreEval(Real t)
{
}

Real PolynomialFunction::Eval(Real t)
{
  Real sum=0;
  Real tprod = 1.0;
  for(size_t i=0;i<coeffs.size();i++) {
    sum += tprod*coeffs[i];
    tprod *= t;
  }
  return sum;
}

Real PolynomialFunction::Deriv(Real t)
{
  Real sum=0;
  Real tprod = 1.0;
  for(size_t i=1;i<coeffs.size();i++) {
    sum += Real(i)*tprod*coeffs[i];
    tprod *= t;
  }
  return sum;
}

Real PolynomialFunction::Deriv2(Real t)
{
  Real sum=0;
  Real tprod = 1.0;
  for(size_t i=2;i<coeffs.size();i++) {
    sum += Real(i)*Real(i-1)*tprod*coeffs[i];
    tprod *= t;
  }
  return sum;
}

size_t PolynomialFunction::EffectiveDegree() const
{
  size_t n=coeffs.size();
  while(n >= 1) {
    if(coeffs[n-1] != 0) return n;
    n--;
  }
  return 0;
}

Real PolynomialFunction::Coefficient(int term) const
{
  if(term < 0 || term >= (int)coeffs.size()) return 0;
  return coeffs[term];
}

void PolynomialFunction::SetConstant(Real value)
{
  coeffs.resize(1);
  coeffs[0] = value;
}

void PolynomialFunction::SetLinear(Real a,Real b)
{
  coeffs.resize(2);
  coeffs[0] = b;
  coeffs[1] = a;
}

void PolynomialFunction::ScaleTime(Real s)
{
  Real invs = 1.0/s;
  Real invsprod = invs;
  for(size_t i=1;i<coeffs.size();i++) {
    coeffs[i] *= invsprod;
    invsprod *= invs;
  }
}

void PolynomialFunction::OffsetTime(Real off)
{
  FatalError("TODO: offset time");
}

void PolynomialFunction::Add(const PolynomialFunction& a,const PolynomialFunction& b)
{
  if(a.coeffs.size() > b.coeffs.size()) {
    Add(b,a);
    return;
  }
  coeffs.resize(b.EffectiveDegree());
  for(size_t i=0;i<a.coeffs.size();i++) {
    coeffs[i] = a.coeffs[i]+b.coeffs[i];
  }
  for(size_t i=a.coeffs.size();i<coeffs.size();i++) 
    coeffs[i] = b.coeffs[i];
}

void PolynomialFunction::Sub(const PolynomialFunction& a,const PolynomialFunction& b)
{
  if(b.coeffs.size() > a.coeffs.size()) {
    Sub(b,a);
    for(size_t i=0;i<coeffs.size();i++)
      coeffs[i] = -coeffs[i];
    return;
  }
  coeffs.resize(a.EffectiveDegree());
  for(size_t i=0;i<b.coeffs.size();i++) {
    coeffs[i] = a.coeffs[i]-b.coeffs[i];
  }
  for(size_t i=b.coeffs.size();i<coeffs.size();i++) 
    coeffs[i] = a.coeffs[i];
}

void PolynomialFunction::Mul(const PolynomialFunction& a,const PolynomialFunction& b)
{
  FatalError("TODO: mul");
}

/*
bool PolynomialFunction::Read(File& f);
bool PolynomialFunction::Write(File& f) const;
*/

